#include "robmath.h"
#include<math.h>


double  Norm(const RobotVector v) {
	return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}
double Dot(const RobotVector u, const RobotVector v) {
	return  u[0] * v[0] + u[1] * v[1] + u[2] * v[2];
}

void UnitVector(const RobotVector u,RobotVector v){
	double norm_vector;
	norm_vector = Norm(u);
	if (norm_vector != 0)
	{
		for (int i=0;i<3;i++)
		{
			v[i] = u[i]/norm_vector;
		}
	}
}

void Cross(const RobotVector u, const RobotVector v, RobotVector w) {
	w[0] = u[1]*v[2] - u[2]*v[1];
	w[1] = u[2]*v[0] - u[0]*v[2];
	w[2] = u[0]*v[1] - u[1]*v[0];
}


void EulerZYX2Rot(const RobotEulerZYX* ptr_euler_zyx, RobotRotationMatrix rot) {

	double euler_z, euler_y, euler_x;
	double Cx, Sx, Cy, Sy, Cz, Sz;

	euler_z = ptr_euler_zyx->euler_z;
	euler_y = ptr_euler_zyx->euler_y;
	euler_x = ptr_euler_zyx->euler_x;

	Cx = cos(euler_x);
	Sx = sin(euler_x);
	Cy = cos(euler_y);
	Sy = sin(euler_y);
	Cz = cos(euler_z);
	Sz = sin(euler_z);

	rot[0][0] = Cz * Cy;
	rot[0][1] = -Sz * Cx + Cz * Sy*Sx;
	rot[0][2] = Sz * Sx + Cz * Sy*Cx;
	rot[1][0] = Sz * Cy;
	rot[1][1] = Cz * Cx + Sz * Sy*Sx;
	rot[1][2] = -Cz * Sx + Sz * Sy*Cx;
	rot[2][0] = -Sy;
	rot[2][1] = Cy * Sx;
	rot[2][2] = Cy * Cx;

}


void Quaternion2Rot(const RobotQuaternion* ptr_quaternion, RobotRotationMatrix rot) {

	double q0, q1, q2, q3;

	q0 = ptr_quaternion->q0;
	q1 = ptr_quaternion->q1;
	q2 = ptr_quaternion->q2;
	q3 = ptr_quaternion->q3;

	rot[0][0] = 1 - 2 * (q2*q2 + q3 * q3);
	rot[0][1] = -2 * q0*q3 + 2 * q1*q2;
	rot[0][2] = 2 * q0*q2 + 2 * q1*q3;
	rot[1][0] = 2 * q0*q3 + 2 * q1*q2;
	rot[1][1] = 1 - 2 * (q1*q1 + q3 * q3);
	rot[1][2] = -2 * q0*q1 + 2 * q2*q3;
	rot[2][0] = -2 * q0*q2 + 2 * q1*q3;
	rot[2][1] = 2 * q0*q1 + 2 * q2*q3;
	rot[2][2] = 1 - 2 * (q1*q1 + q2*q2);
}

void RollPitchYaw2Rot(const RobotRollPitchYaw* ptr_rpy, RobotRotationMatrix rot) {

	RobotEulerZYX euler_zyx;
	euler_zyx.euler_z = ptr_rpy->roll;
	euler_zyx.euler_y = ptr_rpy->pitch;
	euler_zyx.euler_x = ptr_rpy->yaw;

	EulerZYX2Rot(&euler_zyx, rot);
}


void Rot2EulerZYX(const RobotRotationMatrix rot, RobotEulerZYX* ptr_euler_zyx) {
	/*
	% alpha(z), beta(y), gamma(x)
	% Rotating Axis Euler Angles
	% Because rotation matrix to EulerZYX is ont one - to - one mapping
	% we need to restrict beta value into - pi / 2~pi / 2
	% alpha: -pi~pi
	% beta : -pi / 2~pi / 2, cos(beta) > 0
	% gamma: -pi~pi
	%  Calpha*Cbeta - Salpha * Cgamma + Calpha * Sbeta*Sgamma  Salpha*Sgammna + Calpha * Sbeta*Cgamma
	%  Salpha*Cbeta      Calpha*Cgamma + Salpha * Sbeta*Sgamma - Calpha * Sgammna + Salpha * Sbeta*Cgamma
	%  -Sbeta            Cbeta*Sgamma                          Cbeta*Cgamma
	%  Singularity : beta = +/ -pi / 2
	*/

	double R11, R12, R13, R21, R22, R23, R31, R32, R33;
	double Cb;
	double alpha, beta, gamma, delta;

	R11 = rot[0][0];
	R12 = rot[0][1];
	R13 = rot[0][2];
	R21 = rot[1][0];
	R22 = rot[1][1];
	R23 = rot[1][2];
	R31 = rot[2][0];
	R32 = rot[2][1];
	R33 = rot[2][2];

	Cb = sqrt(R32*R32 + R33 * R33);
	if (fabs(Cb) < ROBOT_EPS) {
		if (R31 > 0) {
			beta = -KIN_PI / 2;
			delta = atan2(-R12, -R13);
			alpha = delta / 2;
			gamma = delta / 2;
		}
		else {
			beta = KIN_PI / 2;
			delta = atan2(R12,R13);
			alpha = delta / 2;
			gamma = -delta / 2;
		}
	}
	else {
		beta = atan2(-R31,Cb);
		alpha = atan2(R21 / Cb, R11 / Cb);
		gamma = atan2(R32 / Cb, R33 / Cb);
	}

	ptr_euler_zyx->euler_z = alpha;
	ptr_euler_zyx->euler_y = beta;
	ptr_euler_zyx->euler_x = gamma;


}
void Rot2Quaternion(const RobotRotationMatrix rot, RobotQuaternion* ptr_quaternion){
	/*
	% Q = q0 + q1 * i + q2 * j + q3 * k
	% 1 - 2 * (q2 ^ 2 + q3 ^ 2) - 2 * q0*q3 + 2 * q1*q2  2 * q0*q2 + 2 * q1*q3
	% R = [2 * q0*q3 + 2 * q1*q2   1 - 2 * (q1 ^ 2 + q3 ^ 2) - 2 * q0*q1 + 2 * q2*q3]
	% -2 * q0*q2 + 2 * q1*q3  2 * q0*q1 + 2 * q2*q3   1 - 2 * (q1 ^ 2 + q2 ^ 2)
	*/
	double R11, R12, R13, R21, R22, R23, R31, R32, R33;
	double Tr,S,N;
	double q0, q1, q2, q3;

	R11 = rot[0][0];
	R12 = rot[0][1];
	R13 = rot[0][2];
	R21 = rot[1][0];
	R22 = rot[1][1];
	R23 = rot[1][2];
	R31 = rot[2][0];
	R32 = rot[2][1];
	R33 = rot[2][2];

	Tr = 1 + R11 + R22 + R33;

	if (Tr > 0) {
		S = 2 * sqrt(1 + R11 + R22 + R33);

		q0 = S / 4;
		q1 = (R32 - R23) / S;
		q2 = (R13 - R31) / S;
		q3 = (R21 - R12) / S;
	}
	else {
		if (R11 >= R22 && R11 >= R33) {
			S = 2 * sqrt(1 + R11 - R22 - R33);
			
			q0 = (R32 - R23) / S;
			q1 = S / 4;
			q2 = (R12 + R21) / S;
			q3 = (R13 + R31) / S;
	
		}
		else {
			if (R22 >= R33 && R22 >= R11) {
				S = 2 * sqrt(1+R22-R11-R33);
				
				q0 = (R13 - R31) / S;
				q1 = (R12 + R21) / S;
				q2 = S / 4;
				q3 = (R23 + R32) / S;
			}
			else {
				if (R33 > R22 && R33 >= R11) {
					S = 2 * sqrt(1 + R33 - R11 - R22);

					q0 = (R21 - R12) / S;
					q1 = (R13 + R31) / S;
					q2 = (R23 + R32) / S;
					q3 = S / 4;
				}
			}
		}	
	}

	N = sqrt(q0*q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 = q0 / N;
	q1 = q1 / N;
	q2 = q2 / N;
	q3 = q3 / N;

	ptr_quaternion->q0 = q0;
	ptr_quaternion->q1 = q1;
	ptr_quaternion->q2 = q2;
	ptr_quaternion->q3 = q3;

}
void Rot2RollPitchYaw(const RobotRotationMatrix rot, RobotRollPitchYaw* ptr_rpy) {

	RobotEulerZYX euler_zyx;

	Rot2EulerZYX(rot, &euler_zyx);
	ptr_rpy->roll = euler_zyx.euler_z;
	ptr_rpy->pitch = euler_zyx.euler_y;
	ptr_rpy->yaw = euler_zyx.euler_x;

}
void  Quaternion2EulerZYX(const RobotQuaternion* ptr_quaternion, RobotEulerZYX* ptr_euler_zyx){
	RobotRotationMatrix rot;
	Quaternion2Rot(ptr_quaternion,  rot);
	Rot2EulerZYX(rot,ptr_euler_zyx);
}



void Hat(const RobotVector w, RobotMatrix3x3 hat_w) {
	hat_w[0][0] = 0;
	hat_w[0][1] = -w[2];
	hat_w[0][2] = -w[1];
	hat_w[1][0] = w[2];
	hat_w[1][1] = 0;
	hat_w[1][2] = -w[0];
	hat_w[2][0] = -w[1];
	hat_w[2][1] = w[0];
	hat_w[2][2] = 0;

}
void Expr(const RobotVector w, double theta, RobotRotationMatrix rot) {
	double n = w[0] * w[0] + w[1] * w[1] + w[2] * w[2];
	if (n<ROBOT_EPS) {
		rot[0][0] = 1;
		rot[0][1] = 0;
		rot[0][2] = 0;
		rot[1][0] = 0;
		rot[1][1] = 1;
		rot[1][2] = 0;
		rot[2][0] = 0;
		rot[2][1] = 0;
		rot[2][2] = 1;
	}
	else {
		RobotMatrix3x3 hat_w;
		RobotMatrix3x3 hat_w_square;
		double alpha = sin(n*theta) / n;
		double beta = (1 - cos(n*theta)) / n / n;
		short row, col;
		Hat(w, hat_w);
		for (row = 0; row < 3; row++) {
			for (col = 0; col < 3; col++) {
				hat_w_square[row][col] = hat_w[row][0] * hat_w[0][col] + hat_w[row][1] * hat_w[1][col] + hat_w[row][2] * hat_w[2][col];
			}
		}
		rot[0][0] = 1 + hat_w[0][0] * alpha + hat_w_square[0][0] * beta;
		rot[0][1] = 0 + hat_w[0][1] * alpha + hat_w_square[0][1] * beta;
		rot[0][2] = 0 + hat_w[0][2] * alpha + hat_w_square[0][2] * beta;
		rot[1][0] = 0 + hat_w[1][0] * alpha + hat_w_square[1][0] * beta;
		rot[1][1] = 1 + hat_w[1][1] * alpha + hat_w_square[1][1] * beta;
		rot[1][2] = 0 + hat_w[1][2] * alpha + hat_w_square[1][2] * beta;
		rot[2][0] = 0 + hat_w[2][0] * alpha + hat_w_square[2][0] * beta;
		rot[2][1] = 0 + hat_w[2][1] * alpha + hat_w_square[2][1] * beta;
		rot[2][2] = 1 + hat_w[2][2] * alpha + hat_w_square[2][2] * beta;
	}
	
}
void Expg(const RobotTwist xi, double theta, RobotTransformationMatrix g) {
	RobotVector v;
	RobotVector w;
	double n;
	v[0] = xi[0];
	v[1] = xi[1];
	v[2] = xi[2];
	w[0] = xi[3];
	w[1] = xi[4];
	w[2] = xi[5];
	n = w[0] * w[0] + w[1] * w[1] + w[2] * w[2];
	if (n < ROBOT_EPS) {
		g[0][0] = 1;
		g[0][1] = 0;
		g[0][2] = 0;
		g[1][0] = 0;
		g[1][1] = 1;
		g[1][2] = 0;
		g[2][0] = 0;
		g[2][1] = 0;
		g[2][2] = 1;
		g[0][3] = theta * v[0];
		g[1][3] = theta * v[1];
		g[2][3] = theta * v[2];
		g[3][0] = 0;
		g[3][1] = 0;
		g[3][2] = 0;
		g[3][3] = 1;
	}
	else {
		RobotVector pos;
		RobotRotationMatrix rot;
		int i;

		theta = theta * n;
		for (i = 0; i < 3; i++) {
			v[i] = v[i] / n;
			w[i] = w[i] / n;
		}
		Expr(w, theta, rot);

		{
			RobotMatrix3x3 hat_w;
			RobotMatrix3x3 tmp1, tmp2;
			Hat(w, hat_w);
			tmp1[0][0] = (1 - rot[0][0]) * hat_w[0][0] + (0 - rot[0][1])*hat_w[1][0] + (0 - rot[0][2])*hat_w[2][0];
			tmp1[0][1] = (1 - rot[0][0]) * hat_w[0][1] + (0 - rot[0][1])*hat_w[1][1] + (0 - rot[0][2])*hat_w[2][1];
			tmp1[0][2] = (1 - rot[0][0]) * hat_w[0][2] + (0 - rot[0][1])*hat_w[1][2] + (0 - rot[0][2])*hat_w[2][2];
			tmp1[1][0] = (0 - rot[1][0]) * hat_w[0][0] + (1 - rot[1][1])*hat_w[1][0] + (0 - rot[1][2])*hat_w[2][0];
			tmp1[1][1] = (0 - rot[1][0]) * hat_w[0][1] + (1 - rot[1][1])*hat_w[1][1] + (0 - rot[1][2])*hat_w[2][1];
			tmp1[1][2] = (0 - rot[1][0]) * hat_w[0][2] + (1 - rot[1][1])*hat_w[1][2] + (0 - rot[1][2])*hat_w[2][2];
			tmp1[2][0] = (0 - rot[2][0]) * hat_w[0][0] + (0 - rot[2][1])*hat_w[1][0] + (1 - rot[2][2])*hat_w[2][0];
			tmp1[2][1] = (0 - rot[2][0]) * hat_w[0][1] + (0 - rot[2][1])*hat_w[1][1] + (1 - rot[2][2])*hat_w[2][1];
			tmp1[2][2] = (0 - rot[2][0]) * hat_w[0][2] + (0 - rot[2][1])*hat_w[1][2] + (1 - rot[2][2])*hat_w[2][2];

			tmp2[0][0] = w[0] * w[0];
			tmp2[0][1] = w[0] * w[1];
			tmp2[0][2] = w[0] * w[2];
			tmp2[1][0] = w[1] * w[0];
			tmp2[1][1] = w[1] * w[1];
			tmp2[1][2] = w[1] * w[2];
			tmp2[2][0] = w[2] * w[0];
			tmp2[2][1] = w[2] * w[1];
			tmp2[2][2] = w[2] * w[2];

			pos[0] = tmp1[0][0] * v[0] + tmp1[0][1] * v[1] + tmp1[0][2] * v[2] + tmp2[0][0] * v[0] * theta + tmp2[0][1] * v[1] * theta + tmp2[0][2] * v[2] * theta;
			pos[1] = tmp1[1][0] * v[0] + tmp1[1][1] * v[1] + tmp1[1][2] * v[2] + tmp2[1][0] * v[0] * theta + tmp2[1][1] * v[1] * theta + tmp2[1][2] * v[2] * theta;
			pos[2] = tmp1[2][0] * v[0] + tmp1[2][1] * v[1] + tmp1[2][2] * v[2] + tmp2[2][0] * v[0] * theta + tmp2[2][1] * v[1] * theta + tmp2[2][2] * v[2] * theta;
		}
		g[0][0] = rot[0][0];
		g[0][1] = rot[0][1];
		g[0][2] = rot[0][2];
		g[1][0] = rot[1][0];
		g[1][1] = rot[1][1];
		g[1][2] = rot[1][2];
		g[2][0] = rot[2][0];
		g[2][1] = rot[2][1];
		g[2][2] = rot[2][2];
		g[0][3] = pos[0];
		g[1][3] = pos[1];
		g[2][3] = pos[2];
		g[3][0] = 0;
		g[3][1] = 0;
		g[3][2] = 0;
		g[3][3] = 1;
		
	}
		
}
void Logr(const RobotRotationMatrix rot, RobotVector w, double *ptr_theta) {
	double trace = rot[0][0] + rot[1][1] + rot[2][2];
	if (fabs(trace - 3) < ROBOT_EPS) {
		w[0] = 0;
		w[1] = 0;
		w[2] = 0;
		*ptr_theta = 0;
	}
	else if(fabs(trace - (-1) )<ROBOT_EPS) {
		*ptr_theta = KIN_PI;
		if (fabs(rot[0][0] - 1) < ROBOT_EPS) {
			w[0] = 1;
			w[1] = 0;
			w[2] = 0;
		}
		else if (fabs(rot[1][1] - 1) < ROBOT_EPS) {
			w[0] = 0;
			w[1] = 1;
			w[2] = 0;
		}
		else if (fabs(rot[2][2] - 1) < ROBOT_EPS) {
			w[0] = 0;
			w[1] = 0;
			w[2] = 1;
		}
	}
	else {
		*ptr_theta = acos((trace - 1) / 2);
		if (fabs(*ptr_theta) < ROBOT_EPS) {
			w[0] = 0;
			w[1] = 0;
			w[2] = 0;
			*ptr_theta = 0;
		}
		else {
			double st = sin(*ptr_theta);
			w[0] = (rot[2][1] - rot[1][2]) / (2 * st);
			w[1] = (rot[0][2] - rot[2][0]) / (2 * st);
			w[2] = (rot[1][0] - rot[0][1]) / (2 * st);
		}
	}
}
void Logg(const RobotTransformationMatrix g, RobotTwist xi, double *ptr_theta) {
	RobotRotationMatrix rot;
	RobotVector pos;
	RobotVector w,v;
	rot[0][0] = g[0][0];
	rot[0][1] = g[0][1];
	rot[0][2] = g[0][2];
	rot[1][0] = g[1][0];
	rot[1][1] = g[1][1];
	rot[1][2] = g[1][2];
	rot[2][0] = g[2][0];
	rot[2][1] = g[2][1];
	rot[2][2] = g[2][2];
	pos[0] = g[0][3];
	pos[1] = g[1][3];
	pos[2] = g[2][3];
	Logr(rot, w, ptr_theta);
	if (fabs(*ptr_theta) < ROBOT_EPS) {
		*ptr_theta = sqrt(pos[0] * pos[0] + pos[1] * pos[1] + pos[2] * pos[2]);
		if (fabs(*ptr_theta) < ROBOT_EPS) {
			v[0] = 0;
			v[1] = 0;
			v[2] = 0;
		}
		else {
			v[0] = pos[0] / (*ptr_theta);
			v[1] = pos[1] / (*ptr_theta);
			v[2] = pos[2] / (*ptr_theta);
		}
	}
	else {
		RobotMatrix3x3 inv_a, hat_w, hat_w_square;
		double alpha;
		short row, col;
		Hat(w, hat_w);
		for (row = 0; row < 3; row++) {
			for (col = 0; col < 3; col++) {
				hat_w_square[row][col] = hat_w[row][0] * hat_w[0][col] + hat_w[row][1] * hat_w[1][col] + hat_w[row][2] * hat_w[2][col];
			}
		}
		hat_w_square[0][0] = hat_w[0][0] * hat_w[0][0] + hat_w[0][1] * hat_w[1][0] + hat_w[0][2] * hat_w[2][0];

		alpha = (2 * sin(*ptr_theta) - (*ptr_theta)*(1 + cos(*ptr_theta))) / (2 * (*ptr_theta)*sin(*ptr_theta));
		inv_a[0][0] = 1 / (*ptr_theta) - hat_w[0][0] / 2 + hat_w_square[0][0] * alpha;
		inv_a[0][1] = 0 / (*ptr_theta) - hat_w[0][1] / 2 + hat_w_square[0][1] * alpha;
		inv_a[0][2] = 0 / (*ptr_theta) - hat_w[0][2] / 2 + hat_w_square[0][2] * alpha;
		inv_a[1][0] = 0 / (*ptr_theta) - hat_w[1][0] / 2 + hat_w_square[1][0] * alpha;
		inv_a[1][1] = 1 / (*ptr_theta) - hat_w[1][1] / 2 + hat_w_square[1][1] * alpha;
		inv_a[1][2] = 0 / (*ptr_theta) - hat_w[1][2] / 2 + hat_w_square[1][2] * alpha;
		inv_a[2][0] = 0 / (*ptr_theta) - hat_w[2][0] / 2 + hat_w_square[2][0] * alpha;
		inv_a[2][1] = 0 / (*ptr_theta) - hat_w[2][1] / 2 + hat_w_square[2][1] * alpha;
		inv_a[2][2] = 1 / (*ptr_theta) - hat_w[2][2] / 2 + hat_w_square[2][2] * alpha;
		v[0] = inv_a[0][0] * pos[0] + inv_a[0][1] * pos[1] + inv_a[0][2] * pos[2];
		v[1] = inv_a[1][0] * pos[0] + inv_a[1][1] * pos[1] + inv_a[1][2] * pos[2];
		v[2] = inv_a[2][0] * pos[0] + inv_a[2][1] * pos[1] + inv_a[2][2] * pos[2];

	}
	xi[0] = v[0];
	xi[1] = v[1];
	xi[2] = v[2];
	xi[3] = w[0];
	xi[4] = w[1];
	xi[5] = w[2];
		

}
void AdjointMatrix(const RobotTransformationMatrix g, RobotMatrix6x6 adg) {
	RobotRotationMatrix rot;
	RobotVector pos;
	RobotMatrix3x3 hat_pos;
	short i,j;
	Hat(pos, hat_pos);
	rot[0][0] = g[0][0];
	rot[0][1] = g[0][1];
	rot[0][2] = g[0][2];
	rot[1][0] = g[1][0];
	rot[1][1] = g[1][1];
	rot[1][2] = g[1][2];
	rot[2][0] = g[2][0];
	rot[2][1] = g[2][1];
	rot[2][2] = g[2][2];
	pos[0] = g[0][3];
	pos[1] = g[1][3];
	pos[2] = g[2][3];

	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			adg[i][j] = rot[i][j];
			adg[i + 3][j+3] = rot[i][j];
			adg[i + 3][j] = 0;
			adg[i][j + 3] = hat_pos[i][0] * rot[0][j] + hat_pos[i][1] * rot[1][j] + hat_pos[i][2] * rot[2][j];
		}
	}


}
void AdjointInverseMatrix(const RobotTransformationMatrix g, RobotMatrix6x6 adig) {
	RobotRotationMatrix rott;
	RobotVector pos;
	RobotMatrix3x3 hat_pos;
	short i, j;
	Hat(pos, hat_pos);
	rott[0][0] = g[0][0];
	rott[0][1] = g[1][0];
	rott[0][2] = g[2][0];
	rott[1][0] = g[0][1];
	rott[1][1] = g[1][1];
	rott[1][2] = g[2][1];
	rott[2][0] = g[0][2];
	rott[2][1] = g[1][2];
	rott[2][2] = g[2][2];
	pos[0] = g[0][3];
	pos[1] = g[1][3];
	pos[2] = g[2][3];

	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			adig[i][j] = rott[i][j];
			adig[i + 3][j + 3] = rott[i][j];
			adig[i + 3][j] = 0;
			adig[i][j + 3] = rott[i][0] * hat_pos[0][j] + rott[i][1] * hat_pos[1][j] + rott[i][2] * hat_pos[2][j];
			adig[i][j + 3] = -adig[i][j + 3];
		}
	}

}
void AdjointTwist(const  RobotTwist xi, RobotMatrix6x6 adx) {
	RobotVector w, v;
	w[0] = xi[0];
	w[1] = xi[1];
	w[2] = xi[2];
	v[0] = xi[3];
	v[1] = xi[4];
	v[2] = xi[5];

	adx[0][0] = 0;
	adx[0][1] = -w[2];
	adx[0][2] = -w[1];
	adx[1][0] = w[2];
	adx[1][1] = 0;
	adx[1][2] = -w[0];
	adx[2][0] = -w[1];
	adx[2][1] = w[0];
	adx[2][2] = 0;

	adx[3][3] = 0;
	adx[3][4] = -w[2];
	adx[3][5] = -w[1];
	adx[4][3] = w[2];
	adx[4][4] = 0;
	adx[4][5] = -w[0];
	adx[5][3] = -w[1];
	adx[5][4] = w[0];
	adx[5][5] = 0;

	adx[0][3] = 0;
	adx[0][4] = -v[2];
	adx[0][5] = v[1];
	adx[1][3] = v[2];
	adx[1][4] = 0;
	adx[1][5] = -v[0];
	adx[2][3] = -v[1];
	adx[2][4] = v[0];
	adx[2][5] = 0;

	adx[3][0] = 0;
	adx[3][1] = 0;
	adx[3][2] = 0;
	adx[4][0] = 0;
	adx[4][1] = 0;
	adx[4][2] = 0;
	adx[5][0] = 0;
	adx[5][1] = 0;
	adx[5][2] = 0;
}
void QInverse(const RobotQuaternion* ptr_q, RobotQuaternion* ptr_inv_q) {
	ptr_inv_q->q0 = ptr_q->q0;
	ptr_inv_q->q1 = -ptr_q->q1;
	ptr_inv_q->q2 = -ptr_q->q2;
	ptr_inv_q->q3 = -ptr_q->q3;
}
void QLog(const RobotQuaternion* ptr_q, RobotQuaternion* ptr_log_q) {
	double nq, nv; 

	nq = sqrt(ptr_q->q0 * ptr_q->q0 + ptr_q->q1*ptr_q->q1 + ptr_q->q2*ptr_q->q2 + ptr_q->q3 * ptr_q->q3);
	ptr_log_q->q0 = log(nq);
	nv = sqrt( ptr_q->q1*ptr_q->q1 + ptr_q->q2*ptr_q->q2 + ptr_q->q3 * ptr_q->q3);

	if (nv > ROBOT_EPS) {
		double alpha = acos(ptr_q->q0 / nq);
		ptr_log_q->q1 = ptr_q->q1 / nv * alpha;
		ptr_log_q->q2 = ptr_q->q2 / nv * alpha;
		ptr_log_q->q3 = ptr_q->q3 / nv * alpha;

	}
	else {
		ptr_log_q->q1 = 0;
		ptr_log_q->q2 = 0;
		ptr_log_q->q3 = 0;
	}
}
void QMultiply(const RobotQuaternion* ptr_p, const RobotQuaternion* ptr_q, RobotQuaternion* ptr_pxq) {
	RobotVector v;
	v[2] = ptr_p->q1 * ptr_q->q2 - ptr_p->q2 * ptr_q->q1;
	v[0] = ptr_p->q2 * ptr_q->q3 - ptr_p->q3 * ptr_q->q2;
	v[1] = ptr_p->q3 * ptr_q->q1 - ptr_p->q1 * ptr_q->q3;
	ptr_pxq->q0 = ptr_p->q0 * ptr_q->q0 - (ptr_p->q1*ptr_q->q1 + ptr_p->q2*ptr_q->q2 + ptr_p->q3*ptr_q->q3);
	ptr_pxq->q1 = ptr_p->q0 * ptr_q->q1 + ptr_q->q0 * ptr_p->q1 + v[0];
	ptr_pxq->q2 = ptr_p->q0 * ptr_q->q2 + ptr_q->q0 * ptr_p->q2 + v[1];
	ptr_pxq->q3 = ptr_p->q0 * ptr_q->q3 + ptr_q->q0 * ptr_p->q3 + v[2];

}

void QFrameMultiply(const RobotMatrix4x4 matrix1,const RobotMatrix4x4 matrix2,RobotMatrix4x4 result){
	for (int i = 0;i < 4;i++)
	{
		for (int j = 0;j < 4;j++)
		{
			result[i][j] = 0.0;
			for (int k = 0;k < 4;k++)
			{
				result[i][j] = result[i][j] + matrix1[i][k] * matrix2[k][j];
			}
		}
	}
}

void ConvertVectorToFrame(const bool euler_or_rpy,const RobotVectorSix vector,RobotFrame4x4 frame){

	RobotEulerZYX euler_vector;
	RobotRotationMatrix rot_matrix;
	RobotRollPitchYaw rpy_vector;

 	if (euler_or_rpy == true)
 	{
		euler_vector.euler_x = vector[3];
		euler_vector.euler_y = vector[4];
		euler_vector.euler_z = vector[5];
		EulerZYX2Rot(&euler_vector,rot_matrix);	
 	} 
 	else
 	{
		rpy_vector.roll  = vector[3];
		rpy_vector.pitch = vector[4];
		rpy_vector.yaw   = vector[5];
		RollPitchYaw2Rot(&rpy_vector,rot_matrix);
 	}
	for (int i = 0;i < 3;i++)
	{
		for (int j = 0;j < 3;j++)
		{
			frame[i][j] = rot_matrix[i][j];
		}		
	}	
	frame[3][0] = 0.0;
	frame[3][1] = 0.0;
	frame[3][2] = 0.0;
	frame[3][3] = 1.0;
	frame[0][3] = vector[0];
	frame[1][3] = vector[1];
	frame[2][3] = vector[2];
}
void ConvertFrameToVector(bool euler_or_rpy,const RobotFrame4x4 frame,RobotVectorSix vector){
	
	RobotEulerZYX euler_vector;
	RobotRotationMatrix rot_matrix;
	RobotRollPitchYaw rpy_vector;

	for (int i = 0;i < 3;i++)
	{
		for (int j = 0;j < 3;j++)
		{
			rot_matrix[i][j] = frame[i][j];
		}
	}

	if (euler_or_rpy == true)
	{		
		Rot2EulerZYX(rot_matrix,&euler_vector);
		vector[0] = frame[0][3];
		vector[1] = frame[1][3];
		vector[2] = frame[2][3];
		vector[3] = euler_vector.euler_x;
		vector[4] = euler_vector.euler_y;
		vector[5] = euler_vector.euler_z;
	}
	else 
	{
		Rot2RollPitchYaw(rot_matrix,&rpy_vector);
		vector[0] = frame[0][3];
		vector[1] = frame[1][3];
		vector[2] = frame[2][3];
		vector[3] = rpy_vector.roll;
		vector[4] = rpy_vector.pitch;
		vector[5] = rpy_vector.yaw;
	}
}

void FrameInversion(const RobotFrame4x4 frame,RobotFrame4x4 inv_frame){
 	for (int i = 0;i < 3;i++)
 	{
 		for (int j = 0;j < 3;j++)
 		{
 			inv_frame[i][j] = frame[j][i];
 		}
 		inv_frame[3][i] = 0.0;
 	}
 
 	for (int i = 0;i < 3;i++)
 	{
 		inv_frame[i][3] = -(frame[0][i]*frame[0][3] + frame[1][i]*frame[1][3] + frame[2][i]*frame[2][3]);
 	}
 
 	inv_frame[3][3] = 1.0;
}